//#ident "$Id: CollisionBounds.cpp,v 1.7 2003/07/25 01:01:54 rzr Exp $"
/***************************************************************************
                          CollisionBounds.cpp  -  description
                             -------------------
    begin                : Sun Mar 19 2000
    copyright            : (C) 2000 by 
    email                : 
***************************************************************************/

#include "Private.h"
#include "CollisionBounds.h"
#include "Polygon.h"
#include "Shape3D.h"
#include "Group.h"

#define BOUNDS_OVERLAP 1.0f

/* */
CollisionBounds::CollisionBounds(float size, float x, float y, float z) {
  EM_COUT("CollisionBounds() size"<< size <<" pos "<< x <<" "<< y <<" "<< z << endl, 0);
  m_vtxSrc.x = x;
  m_vtxSrc.y = y;
  m_vtxSrc.z = z;
  m_vtxTrans.x = x;
  m_vtxTrans.y = y;
  m_vtxTrans.z = z;
  m_fBoxSize = size;
  m_fRadius = size*EM_SQRT_3*BOUNDS_OVERLAP;
  p_Parent = NULL;
  m_bHasShape3D = false;
}

CollisionBounds::~CollisionBounds() {
  // the shape and polygon are borrowed from the group
  // so the group will delete them
}

/* Adds a polygon to the collision bound.
 * If the the collision bound has children the polygon is
 * added to all of the children. If a leaf collision bounds
 * surrounds a polygon, there is no need to add it to
 * neighbouring bounds. This function returns true if
 * if a leaf fully surrounds a polygon. */
bool CollisionBounds::addSurround(Polygon3D* p) {
  EmAssert(p != NULL, "CollisionBounds::add polygon NULL");
  EM_COUT("CollisionBounds::add" << endl, 0);
  // add polygons only to leafs
  if (m_vCollisionBounds.size() > 0) {
    vector<CollisionBounds*>::iterator iter = m_vCollisionBounds.begin();
    vector<CollisionBounds*>::iterator end = m_vCollisionBounds.end();
    for (; iter != end; ++iter) {
      if ((*iter)->addSurround(p)) return true;
    }
  } else {
    // add surrounded polygon to bounds
    if (this->surround(p) == 2) {
      m_vPolygon.push_back(p);
      return true;
    }
  }
  return false;
}

void CollisionBounds::addIntersect(Polygon3D* p) {
  EmAssert(p != NULL, "CollisionBounds::add polygon NULL");
  EM_COUT("CollisionBounds::add" << endl, 0);
  // add polygons only to leafs
  if (m_vCollisionBounds.size() > 0) {
    vector<CollisionBounds*>::iterator iter = m_vCollisionBounds.begin();
    vector<CollisionBounds*>::iterator end = m_vCollisionBounds.end();
    for (; iter != end; ++iter) {
      (*iter)->addIntersect(p);
    }
  } else {
    // add intersection polygon to bounds
    if (this->intersect(p)) {
      m_vPolygon.push_back(p);
    }
  }
}

// TODO
/* Does a fast conservative polygon-box intersection test.
 * May return intersection == true even if polygon-box does not intersect. */
int CollisionBounds::intersect(Polygon3D* poly) {
  if (poly == NULL) return 0;
  if (poly->p_Shape3D == NULL) return 0;
  if (poly->m_vIndex.size() == 0) return 0;
  // a variable is true if all vertices is outside a plane created by
  // a side of the box.
  bool bHighX = true;
  bool bLowX = true;
  bool bHighY = true;
  bool bLowY = true;
  bool bHighZ = true;
  bool bLowZ = true;
  // Check if vertices in polygon is inside box.
  vector<int>::iterator iter = poly->m_vIndex.begin();
  vector<int>::iterator end = poly->m_vIndex.end();
  for ( ; iter != end; iter++) {
    Vertex3D vtx = poly->p_Shape3D->m_vVtxSrc[(*iter)];

    if ( (m_vtxSrc.x - m_fBoxSize*BOUNDS_OVERLAP) < vtx.x+0.01 ) bLowX = false;
    if ( vtx.x-0.01 < (m_vtxSrc.x + m_fBoxSize*BOUNDS_OVERLAP) ) bHighX = false;
    if ( (m_vtxSrc.y - m_fBoxSize*BOUNDS_OVERLAP) < vtx.y+0.01 ) bLowY = false;
    if ( vtx.y-0.01 < (m_vtxSrc.y + m_fBoxSize*BOUNDS_OVERLAP) ) bHighY = false;
    if ( (m_vtxSrc.z - m_fBoxSize*BOUNDS_OVERLAP) < vtx.z+0.01 ) bLowZ = false;
    if ( vtx.z-0.01 < (m_vtxSrc.z + m_fBoxSize*BOUNDS_OVERLAP) ) bHighZ = false;
  }
  EM_COUT("CollisionBounds::intersect() " << bLowX <<" "<< bHighX <<" "<<
          bLowY <<" "<< bHighY <<" "<< bLowZ <<" "<< bHighZ, 0);
  if ( bHighX || bLowX || bHighY || bLowY || bHighZ || bLowZ ) return 0;
  return 1;
}

void CollisionBounds::setParent(Group* g) {
  p_Parent = g;
}

void CollisionBounds::setShape3D(Shape3D* s, int level) {
  if (s == NULL) return;
  m_bHasShape3D = true;
  // Remove all old collision bounds.
  // TODO: Look up the name of the destroy function for vector.
        
  // Create new collision bounds
  this->split(level);

  vector<Polygon3D*>::iterator iter = s->m_vPolygon.begin();
  vector<Polygon3D*>::iterator end = s->m_vPolygon.end();
  for (int a=0; iter != end; iter++, a++) {
    if (!this->addSurround(*iter)) {
      this->addIntersect(*iter);
    }
  }
  this->removeEmpty();
  //    this->printTree();
}

/* Returns 2 if box surround all vertices, returns 1 if box surround
 * at least one vertex, else returns 0. */
int CollisionBounds::surround(Polygon3D* poly) {
  if (poly == NULL) return 0;
  if (poly->p_Shape3D == NULL) return 0;
  if (poly->m_vIndex.size() == 0) return 0;
  bool bFull = true;
  bool bSurround  = false;
  // Check if vertices in polygon is inside box.
  vector<int>::iterator iter = poly->m_vIndex.begin();
  vector<int>::iterator end = poly->m_vIndex.end();
  for ( ; iter != end; iter++) {
    Vertex3D vtx = poly->p_Shape3D->m_vVtxSrc[(*iter)];
    float dx = m_vtxSrc.x - vtx.x;
    float dy = m_vtxSrc.y - vtx.y;
    float dz = m_vtxSrc.z - vtx.z;
    EM_COUT(dx <<" "<< dy <<" "<< dz <<endl, 0);
    if ( dx >= -m_fBoxSize*BOUNDS_OVERLAP && dx <= m_fBoxSize*BOUNDS_OVERLAP &&
         dy >= -m_fBoxSize*BOUNDS_OVERLAP && dy <= m_fBoxSize*BOUNDS_OVERLAP &&
         dz >= -m_fBoxSize*BOUNDS_OVERLAP && dz <= m_fBoxSize*BOUNDS_OVERLAP) {
      bSurround = true;
    } else {
      bFull = false;
    }
  }
  if (bFull) return 2;
  else if (bSurround) return 1;
  return 0;
}


void CollisionBounds::printTree(int level) {
  EM_COUT("T: ", 0);
  int a;
  for (a=0; a<level; ++a) {
    EM_COUT("  ", 0);
  }
  EM_COUT("CollisionBounds " << this->m_vPolygon.size() << " polygons", 0);
  vector<CollisionBounds*>::iterator iter = m_vCollisionBounds.begin();
  vector<CollisionBounds*>::iterator end = m_vCollisionBounds.end();
  for (; iter != end; ++iter) {
    (*iter)->printTree(level+1);
  }
}

bool CollisionBounds::removeEmpty() {
  vector<CollisionBounds*>::iterator iter = m_vCollisionBounds.begin();
  vector<CollisionBounds*>::iterator end = m_vCollisionBounds.end();

  for (; iter != end; ++iter) {
    if ((*iter)->removeEmpty()) {
      CollisionBounds* cb = (*iter);
      m_vCollisionBounds.erase(iter);
      delete(cb);
      // an object is erased, I think we need re-initialize
      iter = m_vCollisionBounds.begin();
      end = m_vCollisionBounds.end();
      EM_COUT("CollisionBounds::removeEmpty - removed a bound", 0);
    }
  }
  return (m_vCollisionBounds.size() == 0 && m_vPolygon.size() == 0);
}

void CollisionBounds::split(int level) {
  int a, b, c;
  if (level <= 1) return;
  EM_COUT("CollisionBounds::split "<< level << endl, 0);

  for (a=-1; a<2; a+=2) {
    for (b=-1; b<2; b+=2) {
      for (c=-1; c<2; c+=2) {
        CollisionBounds* cb = new //!rzr MLK
          CollisionBounds( m_fBoxSize*0.5, 
                           m_vtxSrc.x + a*m_fBoxSize*0.5, 
                           m_vtxSrc.y + b*m_fBoxSize*0.5,
                           m_vtxSrc.z + c*m_fBoxSize*0.5);
        cb->setParent(p_Parent);
        m_vCollisionBounds.push_back(cb);
      }
    }
  }
  vector<CollisionBounds*>::iterator iter = m_vCollisionBounds.begin();
  vector<CollisionBounds*>::iterator end = m_vCollisionBounds.end();
  for ( ; iter != end; iter++) {
    (*iter)->split(level-1);
  }
}


void CollisionBounds::transform(const Matrix & mtx) {
  vector<CollisionBounds*>::iterator iter = m_vCollisionBounds.begin();
  vector<CollisionBounds*>::iterator end = m_vCollisionBounds.end();
  for ( ; iter != end; ++iter) {
    (*iter)->transform(mtx);
  }
  //EMATH_APPLYMATRIX(mtx, m_vtxSrc, m_vtxTrans);
  EMath::applyMatrix(mtx, m_vtxSrc, m_vtxTrans);
  EM_COUT("CollisionBounds::transform "<< m_vtxTrans.x <<" "<< m_vtxTrans.y <<" "<< m_vtxTrans.z, 0);
}

